/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#define MAP_FILE_NAME "../../res/autmap-small.txt"

#include<stdio.h>
#include<stdlib.h>
#include<math.h>

#include "TOccupancyGrid.h" ///< Occupancy grid class
#include "glutImageGUI.h"   ///< For visualisation
#include "RGBImage.h"
#include "JPEGEncoder.hpp"  ///< For saving JPEG
#include "CScanMeasurementSet.h"

		
float topi(float da){
	if(da>M_PI)while(da>M_PI) da-=2.0*M_PI;
	else if(da<-M_PI)while(da<-M_PI) da+=2.0*M_PI;
	return da;
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}

float randomi(){
	return (2.0 * (float)(rand()%10000) - 1.0)/10000.0;
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////

void printUsage(){
		fprintf(stderr,"\n-----------------------------------------------------------------\n");
		fprintf(stderr,"Plots a batch of scans from a log file\n");
		fprintf(stderr,"Usage: ./plot_scan_measurement [in_file]\n");
		fprintf(stderr,"MACI FILES: ./plot_scan_measurement [in_position] [in_ranging]\n");
		
		fprintf(stderr,"if [in_file] is not given the default file is LnMlib/data/j2b2_measurement.txt\n");
		fprintf(stderr,"\nThe output is saved to 'out_result.jpg'\n");
		fprintf(stderr,"-----------------------------------------------------------------\n\n");
}



int main(int argc, char *argv[]){
		const char *measurement_file_name="../../../data/j2b2_measurement.txt";
		//float grid_size = 30.0; ///< size of the local grid
		float grid_size = 150.0; ///< size of the local grid
		float gresolution = 0.1;
		float filtDist=50;
		// Init Debug lib
		debugInit();
		debugSetGlobalDebugLvl(4);
	
		char *position_file_name=NULL;
		char *ranging_file_name=NULL;
		
		if(argc == 3){
			position_file_name = argv[1];
			ranging_file_name = argv[2];
		}else if(argc == 2){
			measurement_file_name = argv[1];
		}
		else{
			printUsage();
			exit(1);
		}
		
		
		TOccupancyGrid O(grid_size,grid_size,gresolution,Grid::TCoord2D(0,0));
		O.setGridToValue(0.5);
		
		Grid::TScan meas;
		Grid::pose pos;
		
		std::vector< Grid::pose > poses;    ///All poses for previous map
		std::vector< Grid::pose > lposes;    ///All poses for previous map

		int Nx,Ny;	
		O.getDim(Nx,Ny);
		RGBUtil::RGBImage rgb(Nx,Ny);
		
		addImageWindow(Nx, Ny);
		glutImageGUI_Start();
		
		CScanMeasurementSet *dset;
		
		if(position_file_name != NULL){
			dset = new CScanMeasurementSet(position_file_name,ranging_file_name);
		}else{
			dset = new CScanMeasurementSet(measurement_file_name);
		}
		fprintf(stderr,"DATASET size = %d Nummeas=%d\n", dset->size(),dset->angles.size());
		
		Grid::pose lp;
		int cnt=0;
		for(int i=0;i<dset->size();i++){
				if(!dset->getMeasurementInLaserFrame(i, lp, meas)){
					fprintf(stderr,"FAILED TO READ\n");
					exit(1);
				}
				//dset->getMeasurement(i, lp, meas);
				//fprintf(stderr,"Num meas = %d\n",meas.N);
				pos = dset->poses[i];
				poses.push_back(pos);
				lposes.push_back(lp);
				
				meas.filt(filtDist);
				//fprintf(stderr,"Meas size=%d\n",meas.N);
				//for(int j=0;j<meas.N;j++){
				//	fprintf(stderr,"%f %f\n",meas.a[j], meas.r[j] );
			//	}
				
				O.setScanFillProb(lp.x, lp.y ,lp.a,meas,0.3,0.99);
				//usleep(10000);
				if(cnt%50==0){
						rgb.set(O.getDataPtr(), 255, 255, 255);
						
						for(int i=0;i<poses.size();i++){
								Grid::TPixel pix = O.toPixel(Grid::TCoord2D(poses[i].x,poses[i].y));
								rgb.setPixel(pix.x,pix.y, 255,0, 0 );
								
								pix = O.toPixel(Grid::TCoord2D(lposes[i].x,lposes[i].y));
								rgb.setPixel(pix.x,pix.y, 0,255, 0 );
						}
						displayImage(0, rgb.getDataPtr(),Ny,Nx);
				}
				cnt++;
		}
		
		
		fprintf(stderr,"\n ######   THE END - CTRL-C Ends the show   ######\n");
		cnt = 0;
		while(1){
				displayImage(0, rgb.getDataPtr(),Ny,Nx);
				if(cnt ==0){
						saveJPEG("out_result.jpg", rgb.getDataPtr(),Nx,Ny);
						cnt++;
				}
				usleep(1000*100);
				
		}
		
		
	return 0;
}		
